#include "io_module.h"
#include "log_module.h"
#include "alarm_module.h"
#include <rtdevice.h>
#include <board.h>

static io_config_t *io_config = RT_NULL;
static rt_thread_t io_thread = RT_NULL;
static rt_timer_t io_scan_timer = RT_NULL;

/* Hardware pin definitions - need to be adjusted for actual hardware */
static const rt_base_t input_pins[DIGITAL_INPUT_CHANNELS] = {
    GET_PIN(A, 0),  GET_PIN(A, 1),  GET_PIN(A, 2),  GET_PIN(A, 3),
    GET_PIN(A, 4),  GET_PIN(A, 5),  GET_PIN(A, 6),  GET_PIN(A, 7),
    GET_PIN(B, 0),  GET_PIN(B, 1),  GET_PIN(B, 2),  GET_PIN(B, 3)
};

static const rt_base_t output_pins[DIGITAL_OUTPUT_CHANNELS] = {
    GET_PIN(C, 0),  GET_PIN(C, 1),  GET_PIN(C, 2),  GET_PIN(C, 3),
    GET_PIN(C, 4),  GET_PIN(C, 5),  GET_PIN(C, 6),  GET_PIN(C, 7),
    GET_PIN(C, 8),  GET_PIN(C, 9),  GET_PIN(C, 10), GET_PIN(C, 11)
};

/* Debounce state for each input */
static struct {
    debounce_state_t state;
    rt_bool_t last_raw_state;
    uint32_t stable_time;
    uint8_t debounce_count;
} input_debounce[DIGITAL_INPUT_CHANNELS];

static void io_hardware_init(void)
{
    /* Initialize input pins */
    for (int i = 0; i < DIGITAL_INPUT_CHANNELS; i++) {
        rt_pin_mode(input_pins[i], PIN_MODE_INPUT_PULLUP);
    }
    
    /* Initialize output pins */
    for (int i = 0; i < DIGITAL_OUTPUT_CHANNELS; i++) {
        rt_pin_mode(output_pins[i], PIN_MODE_OUTPUT);
        rt_pin_write(output_pins[i], PIN_LOW);
    }
}

static rt_bool_t io_read_hardware_input(uint8_t channel)
{
    if (channel >= DIGITAL_INPUT_CHANNELS) {
        return RT_FALSE;
    }
    
    rt_base_t pin_state = rt_pin_read(input_pins[channel]);
    return (pin_state == PIN_HIGH) ? RT_TRUE : RT_FALSE;
}

static rt_err_t io_write_hardware_output(uint8_t channel, rt_bool_t state)
{
    if (channel >= DIGITAL_OUTPUT_CHANNELS) {
        return RT_ERR_PARAM;
    }
    
    rt_pin_write(output_pins[channel], state ? PIN_HIGH : PIN_LOW);
    return RT_OK;
}

static rt_bool_t input_debounce_filter(uint8_t channel, rt_bool_t current_state)
{
    if (channel >= DIGITAL_INPUT_CHANNELS) {
        return RT_FALSE;
    }
    
    rt_bool_t filtered_state = input_debounce[channel].last_raw_state;
    uint32_t current_time = rt_tick_get();
    
    switch (input_debounce[channel].state) {
        case DEBOUNCE_IDLE:
            if (current_state != filtered_state) {
                input_debounce[channel].state = (current_state) ? DEBOUNCE_RISING : DEBOUNCE_FALLING;
                input_debounce[channel].stable_time = current_time;
                input_debounce[channel].debounce_count = 1;
            }
            break;
            
        case DEBOUNCE_RISING:
        case DEBOUNCE_FALLING:
            if (current_state == filtered_state) {
                /* Back to stable state */
                input_debounce[channel].state = DEBOUNCE_IDLE;
            } else {
                input_debounce[channel].debounce_count++;
                if (current_time - input_debounce[channel].stable_time >= 
                    io_config->inputs[channel].filter_time) {
                    /* Debounce time elapsed, accept new state */
                    filtered_state = current_state;
                    input_debounce[channel].state = DEBOUNCE_STABLE;
                    input_debounce[channel].stable_time = current_time;
                }
            }
            break;
            
        case DEBOUNCE_STABLE:
            if (current_state != filtered_state) {
                input_debounce[channel].state = (current_state) ? DEBOUNCE_RISING : DEBOUNCE_FALLING;
                input_debounce[channel].stable_time = current_time;
                input_debounce[channel].debounce_count = 1;
            }
            break;
    }
    
    input_debounce[channel].last_raw_state = current_state;
    return filtered_state;
}

static rt_bool_t calculate_valid_state(uint8_t channel, rt_bool_t filtered_state)
{
    if (channel >= DIGITAL_INPUT_CHANNELS) {
        return RT_FALSE;
    }
    
    if (io_config->inputs[channel].normally_open) {
        /* Normally open: valid when input is ON */
        return filtered_state;
    } else {
        /* Normally closed: valid when input is OFF */
        return !filtered_state;
    }
}

static void io_scan_timeout(void *parameter)
{
    /* This function runs in timer interrupt context, be quick! */
    rt_bool_t raw_state, filtered_state, valid_state;
    
    /* Scan inputs */
    for (int i = 0; i < DIGITAL_INPUT_CHANNELS; i++) {
        if (!io_config->inputs[i].enabled) {
            continue;
        }
        
        /* Read hardware input */
        raw_state = io_read_hardware_input(i);
        
        /* Apply debounce filter */
        filtered_state = input_debounce_filter(i, raw_state);
        
        /* Calculate valid state based on normally open/closed configuration */
        valid_state = calculate_valid_state(i, filtered_state);
        
        /* Update configuration */
        io_config->inputs[i].current_state = filtered_state;
        io_config->inputs[i].valid_state = valid_state;
    }
    
    /* Control outputs */
    for (int i = 0; i < DIGITAL_OUTPUT_CHANNELS; i++) {
        rt_bool_t output_state = io_config->outputs[i].command_state;
        
        /* Apply normally open/closed inversion */
        if (io_config->outputs[i].normally_open) {
            output_state = !output_state;
        }
        
        /* Write to hardware */
        io_write_hardware_output(i, output_state);
        io_config->outputs[i].current_state = output_state;
    }
}

static void process_io_message(message_t *msg)
{
    if (msg == RT_NULL) return;
    
    switch (msg->msg_type) {
        case MSG_CONTROL_CMD:
            if (msg->data != RT_NULL && msg->data_len == sizeof(io_set_output_t)) {
                io_set_output_t *cmd = (io_set_output_t *)msg->data;
                io_set_output_state(cmd->channel, cmd->state);
            }
            break;
            
        case MSG_SYSTEM_SHUTDOWN:
            /* Set all outputs to safe state */
            io_set_safe_state();
            break;
            
        default:
            break;
    }
}

rt_err_t io_module_init(void)
{
    rt_err_t result;
    
    /* Get configuration pointer */
    io_config = &g_system_config.io;
    
    /* Initialize hardware */
    io_hardware_init();
    
    /* Initialize debounce states */
    for (int i = 0; i < DIGITAL_INPUT_CHANNELS; i++) {
        input_debounce[i].state = DEBOUNCE_IDLE;
        input_debounce[i].last_raw_state = RT_FALSE;
        input_debounce[i].stable_time = 0;
        input_debounce[i].debounce_count = 0;
    }
    
    /* Create scan timer */
    io_scan_timer = rt_timer_create("io_scan", 
                                   io_scan_timeout, 
                                   RT_NULL, 
                                   rt_tick_from_millisecond(io_config->base.update_interval),
                                   RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_SOFT_TIMER);
    if (io_scan_timer == RT_NULL) {
        LOG_E("Failed to create IO scan timer\n");
        return RT_ERR_MEM;
    }
    
    /* Create IO thread */
    io_thread = rt_thread_create("io", 
                                io_thread_entry, 
                                RT_NULL, 
                                1024, 
                                10, 
                                20);
    if (io_thread == RT_NULL) {
        LOG_E("Failed to create IO thread\n");
        rt_timer_delete(io_scan_timer);
        return RT_ERR_MEM;
    }
    
    /* Start scan timer */
    rt_timer_start(io_scan_timer);
    
    LOG_I("IO module initialized successfully\n");
    return RT_OK;
}

void io_thread_entry(void *parameter)
{
    message_t msg;
    
    while (1) {
        /* Wait for messages */
        if (rt_mq_recv(g_msg_queue, &msg, sizeof(msg), RT_WAITING_FOREVER) == RT_EOK) {
            if (msg.dst_module == MODULE_IO) {
                process_io_message(&msg);
            }
        }
    }
}

rt_bool_t io_get_input_state(uint8_t channel)
{
    if (channel >= DIGITAL_INPUT_CHANNELS || io_config == RT_NULL) {
        return RT_FALSE;
    }
    
    return io_config->inputs[channel].current_state;
}

rt_bool_t io_get_input_valid_state(uint8_t channel)
{
    if (channel >= DIGITAL_INPUT_CHANNELS || io_config == RT_NULL) {
        return RT_FALSE;
    }
    
    return io_config->inputs[channel].valid_state;
}

rt_err_t io_set_output_state(uint8_t channel, rt_bool_t state)
{
    if (channel >= DIGITAL_OUTPUT_CHANNELS || io_config == RT_NULL) {
        return RT_ERR_PARAM;
    }
    
    io_config->outputs[channel].command_state = state;
    return RT_OK;
}

rt_bool_t io_get_output_state(uint8_t channel)
{
    if (channel >= DIGITAL_OUTPUT_CHANNELS || io_config == RT_NULL) {
        return RT_FALSE;
    }
    
    return io_config->outputs[channel].current_state;
}

rt_err_t io_set_safe_state(void)
{
    if (io_config == RT_NULL) {
        return RT_ERR_PARAM;
    }
    
    for (int i = 0; i < DIGITAL_OUTPUT_CHANNELS; i++) {
        io_config->outputs[i].command_state = io_config->outputs[i].safe_state;
    }
    
    return RT_OK;
}

io_config_t *io_get_config(void)
{
    return io_config;
}